#include "my_planner.h"
#include <pluginlib/class_list_macros.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <unistd.h>

// 注册为ROS导航本地规划器插件
PLUGINLIB_EXPORT_CLASS(my_planner::MyPlanner, nav_core::BaseLocalPlanner)

namespace my_planner
{
    // 类静态成员变量声明（需在类外定义）
    tf::TransformListener *tf_listener_;
    costmap_2d::Costmap2DROS *costmap_ros_;
    
    // 构造函数：设置本地化环境（中文支持）
    MyPlanner::MyPlanner()
    {
        setlocale(LC_ALL, "");
    }

    // 析构函数
    MyPlanner::~MyPlanner()
    {
    }

    // 初始化函数：设置规划器名称、TF缓冲区和代价地图
    void MyPlanner::initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)
    {
        ROS_WARN("初始化完成，可以运行");
        tf_listener_ = new tf::TransformListener(); // 创建TF监听器
        costmap_ros_ = costmap_ros;                 // 保存代价地图指针
    }

    // 设置全局路径
    bool MyPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)
    {
        target_index_ = 0;       // 重置目标点索引
        global_plan_ = plan;     // 保存全局路径
        pose_adjusting_ = false; // 初始未进入姿态调整
        goal_reached_ = false;   // 重置到达标志
        return true;
    }

    // 核心函数：计算速度指令
    bool MyPlanner::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
    {
        // === 代价地图可视化处理 ===
        costmap_2d::Costmap2D *costmap = costmap_ros_->getCostmap();
        unsigned char *map_data = costmap->getCharMap();  // 获取原始代价数据
        unsigned int size_x = costmap->getSizeInCellsX(); // 地图X方向大小（像素）
        unsigned int size_y = costmap->getSizeInCellsY(); // 地图Y方向大小（像素）

        // === 路径点处理与碰撞检测 ===
        for (int i = 0; i < global_plan_.size(); i++)
        {
            // 将路径点转换到odom坐标系
            geometry_msgs::PoseStamped pose_odom;
            global_plan_[i].header.stamp = ros::Time(0); // 使用最新可用变换
            tf_listener_->transformPose("odom", global_plan_[i], pose_odom);

            // 转换到代价地图坐标系
            double origin_x = costmap->getOriginX(); 
            double origin_y = costmap->getOriginY();
            double local_x = pose_odom.pose.position.x - origin_x;
            double local_y = pose_odom.pose.position.y - origin_y;
            int map_x = local_x / costmap->getResolution(); // 计算地图坐标
            int map_y = local_y / costmap->getResolution();

            // 检测前方10个点是否在危险区域
            if (i >= target_index_ && i < target_index_ + 10)
            {
                int map_index = map_y * size_x + map_x;
                unsigned char cost = map_data[map_index];
                if (cost >= 253) // 253(禁行)或254(障碍)
                {
                    ROS_WARN("检测到路径点 %d 在危险区域!", i);
                    //return false 让全局规划器重新规划路线
                    return false; // 存在碰撞风险，停止运动
                }
            }
        }

        // === 最终姿态调整逻辑 ===
        int final_index = global_plan_.size() - 1;
        geometry_msgs::PoseStamped pose_final;
        global_plan_[final_index].header.stamp = ros::Time(0);
        tf_listener_->transformPose("base_link", global_plan_[final_index], pose_final);

        double dx = pose_final.pose.position.x;
        double dy = pose_final.pose.position.y;
        double dist = std::sqrt(dx * dx + dy * dy);
        // 当距离目标点<1cm时进入姿态调整阶段
        if (!pose_adjusting_)
        {
            ROS_WARN("final_num = %d",final_num);
            if (final_flag == 1)
            {
                if (dist < 0.001)
                {
                    final_num++;
                    if (final_num == 3)
                    {
                        pose_adjusting_ = true;
                        final_num = 0;
                    }
                }
                else
                    final_num = 0;
            }
        }

        // 姿态调整阶段（旋转到目标朝向）
        if (pose_adjusting_)
        {
            double final_yaw = tf::getYaw(pose_final.pose.orientation);
            ROS_WARN("调整最终姿态, 朝向偏差 = %.2f rad,距离误差=%.2f", final_yaw, dist);

            // 控制策略：线性位置和角度偏差的比例控制
            cmd_vel.linear.x = pose_final.pose.position.x * 1.5;
            cmd_vel.angular.z = final_yaw * 0.5;

            //  放弃姿态调整，节约时间
            if (std::abs(final_yaw) < 10)
            {
                goal_reached_ = true;
                ROS_WARN("到达终点！");
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
            }
            return true;
        }

        // === 常规路径跟踪 ===
        geometry_msgs::PoseStamped target_pose;
        // 寻找前方0.2米以外的目标点
        for (int i = target_index_; i < global_plan_.size(); i++)
        {
            geometry_msgs::PoseStamped pose_base;
            global_plan_[i].header.stamp = ros::Time(0);
            tf_listener_->transformPose("base_link", global_plan_[i], pose_base);

            double dx = pose_base.pose.position.x;
            double dy = pose_base.pose.position.y;
            double dist = std::sqrt(dx * dx + dy * dy);

            // 选择距离机器人>0.3m的第一个点作为目标点
            if (dist > 0.5)
            {
                target_pose = pose_base;
                target_index_ = i;
                break;
            }

            // 如果已到路径末端，直接选择最后一个点
            if (i == global_plan_.size() - 1)
            {
                target_pose = pose_base;
                final_flag = 1;
            }
        }

        speed_control(target_pose.pose.position.x, target_pose.pose.position.y, &last_dx, &last_dy, cmd_vel);
        // // 实验性朝向控制 充电时可开启
        // geometry_msgs::PoseStamped pose_map, pose_base;
        // pose_map.pose.position.x = 1.0;  // 地图固定点
        // pose_map.pose.position.y = -1.5;
        // pose_map.pose.orientation.w = 1.0;
        // pose_map.header.frame_id = "map";
        // pose_map.header.stamp = ros::Time(0);
        // tf_listener_->transformPose("base_link", pose_map, pose_base);
        // cmd_vel.angular.z = pose_base.pose.position.y * 1.0;  // 可能错误的控制逻辑

        return true;
    }

    // 检查是否到达目标
    bool MyPlanner::isGoalReached()
    {
        return goal_reached_;
    }

    // 生成线速度指令（pid控制）
    void MyPlanner::speed_control(double dx, double dy, double *last_dx, double *last_dy, geometry_msgs::Twist &cmd_vel)
    {
        double dist_tar = std::sqrt(dx * dx +dy *dy);
        ROS_WARN("距离误差为%.2f", dist_tar);

        cmd_vel.linear.x = dx * 13 + (dx - *last_dx) * 6;
        cmd_vel.linear.y = dy * 13 + (dy - *last_dy) * 6;

        *last_dx = dx;
        *last_dy = dy;
    } 
} // namespace my_planner
